﻿// 2020/7/1: 首个版本
// 2021/7/7: 新增uSoundSampleProtocols
// 2023/10/11: 补充部分英文注释
// 2024/3/25: 修正注释
// 2025/4/30: 增加usoundSampleConverters

function USoundSensorProperty() {
    this.originX = 0; // [m] X-coordinate of the n-th sensor's position / 传感器原点X坐标
    this.originY = 0; // [m] Y-coordinate of the n-th sensor's position / 传感器原点Y坐标
    this.orientation = 0; // [°] Central heading angle of the n-th sensor / 传感器中心轴朝向
    this.angleRangeA = 0; // [°] α angle range (generally around 100°, with sensor's origin as the center of the circle) / α角范围（一般在100°左右，以OriginX/Y为圆心）
    this.angleRangeB = 0; // [°] β angle range (generally ranges 0~30°) / β角范围（一般在0~30°）
    this.distanceRangeA = 0; // [m] Detect range corresponding to α angle (shorter) / α角对应的探测距离（较短）
    this.distanceRangeB = 0; // [m] Detect range corresponding to β angle (longer) / β角对应的探测距离（较长）
};

function USoundSensorData() {
    this.id = 0; // Sensor ID, starting from 1 / 传感器ID，从1开始
    this.properties = new USoundSensorProperty(); // Sensor properties / 超声波传感器属性
    this.distance = null; // [m] Detected barrier's distance, null indicates no barrier / 障碍物的距离，无障碍物则为空
}

function USoundSample(session, time) {
    this.session = session; // Date or AppoxSessionIdentifier
    this.time = time; // [s] Time offset in session / Session内的相对时间
    this.dataList = []; // Data of each sensor / 各个超声波传感器的数据
}

function convUSoundSampleV1(gs) {
    if (gs.values.length < 1) return null;

    var sensorCount = gs.values[0] == null ? 0 : gs.values[0];
    if (gs.values.length != 1 + 9 * sensorCount) return null;

    var sample = new USoundSample(gs.session, gs.time);
    sample.dataList = Array(sensorCount);
    for (var i = 0; i < sensorCount; i++) {
        var data = new USoundSensorData();
        data.id = gs.values[1 + 9 * i];
        data.distance = gs.values[2 + 9 * i];
        data.properties.originX = gs.values[3 + 9 * i];
        data.properties.originY = gs.values[4 + 9 * i];
        data.properties.orientation = gs.values[5 + 9 * i];
        data.properties.angleRangeA = gs.values[6 + 9 * i];
        data.properties.angleRangeB = gs.values[7 + 9 * i];
        data.properties.distanceRangeA = gs.values[8 + 9 * i];
        data.properties.distanceRangeB = gs.values[9 + 9 * i];
        sample.dataList[i] = data;
    }
    
    return sample;
}

// Interpolator / 样本插值函数
function interpolateUSoundSample(session, time, s1, w1, s2, w2) {
    var sample = new USoundSample(session, time);
    for (var i1 = 0; i1 < s1.dataList.length; i1++) {
        var o1 = s1.dataList[i1];
        var id = o1.id;
        for (var i2 = 0; i2 < s2.dataList.length; i2++) {
            var o2 = s2.dataList[i2];
            if (o2.id != id) continue;
            var data = new USoundSensorData();
            data.id = id;
            data.properties = o1.properties;
            data.distance = o1.distance == null || o2.distance == null ? null : o1.distance * w1 + o2.distance * w2;
            sample.dataList.push(data);
        }
    }
    return sample;
}

// Supported protocols / 超声波传感器样本支持解析的协议
var uSoundSampleProtocols = [
    "usound-sample-v1",
];

// Converters / 通用样本转换函数表
var uSoundSampleConverters = {
    "usound-sample-v1": convUSoundSampleV1,
};

// Use this function to query ultrasound sensor sample at target time. If "interpolated" is true, the interpolated frame is preferred, otherwise the nearest frame / 获取指定session指定时刻的超声波传感器样本，若interpolated为true则优先取插值帧，否则固定为最近帧
function getUSoundSample(session, time, interpolated) {
    var s1 = null, w1 = null, s2 = null, w2 = null;
    var pairV1 = biGetGeneralSamplePair('usound-sample-v1', null, session, time, 1);
    if (pairV1 != null) {
        if (pairV1.sample1 != null) s1 = convUSoundSampleV1(pairV1.sample1);
        if (pairV1.sample2 != null) s2 = convUSoundSampleV1(pairV1.sample2);
        w1 = pairV1.weight1;
        w2 = pairV1.weight2;
    }
    else return null;
    if (s1 == null || s2 == null) {
        return s1 == null ? s2 : s1;
    }
    if (interpolated) {
        return interpolateUSoundSample(session, time, s1, w1, s2, w2);
    }
    else {
        return w1 > w2 ? s1 : s2;
    }
}